/*
用于模拟摇杆消息的JoyPublisher类，支持在站立和行走状态切换
*/
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <sensor_msgs/msg/joy.hpp>
#include <thread>
#include <mutex>
#include <atomic>

// JoyPublisher simulate joystick message, support stance and walk commands
class JoyPublisher {
public:
    explicit JoyPublisher(const rclcpp::Node::SharedPtr& node, double hz = 10.0)
    : node_(node), hz_(hz), running_(false) {
        joy_pub_ = node_->create_publisher<sensor_msgs::msg::Joy>("/joy", 10);
    }

    ~JoyPublisher() {
        stop();
    }

    void publish_stance(double wait_time = 0.0, double duration = 0.5) {
        start_thread([this, wait_time, duration]() { publish_joy_loop(make_stance_msg(), wait_time, duration); });
    }

    void publish_walk(double wait_time = 0.0, double duration = 0.5) {
        start_thread([this, wait_time, duration]() { publish_joy_loop(make_walk_msg(), wait_time, duration); });
    }

    void stop() {
        running_ = false;
        if (thread_.joinable()) thread_.join();
    }

private:
    sensor_msgs::msg::Joy make_empty_msg() {
        sensor_msgs::msg::Joy joy_msg;
        joy_msg.header.stamp = node_->now();
        joy_msg.header.frame_id = "/dev/input/js0";
        joy_msg.axes = std::vector<float>(8, 0.0f);
        joy_msg.buttons = std::vector<int>(16, 0);
        return joy_msg;
    }

    sensor_msgs::msg::Joy make_stance_msg() {
        auto joy_msg = make_empty_msg();
        joy_msg.buttons = {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
        return joy_msg;
    }

    sensor_msgs::msg::Joy make_walk_msg() {
        auto joy_msg = make_empty_msg();
        joy_msg.buttons = {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
        return joy_msg;
    }

    void start_thread(std::function<void()> func) {
        stop();
        running_ = true;
        thread_ = std::thread([this, func]() { func(); });
    }

    void publish_joy_loop(sensor_msgs::msg::Joy joy_msg, double wait_time, double duration) {
        auto period = std::chrono::duration<double>(1.0 / hz_);
        auto start = node_->now() + rclcpp::Duration::from_seconds(wait_time);
        while (running_) {
            double elapsed = (node_->now() - start).seconds();
            if (elapsed > 0 && elapsed < duration) {
                joy_msg.header.stamp = node_->now();
                joy_pub_->publish(joy_msg);
                rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::milliseconds>(period));
            } else if (elapsed >= duration) {
                break;
            }
        }
        joy_pub_->publish(make_empty_msg());
    }

    rclcpp::Node::SharedPtr node_;
    rclcpp::Publisher<sensor_msgs::msg::Joy>::SharedPtr joy_pub_;
    double hz_;
    std::atomic<bool> running_;
    std::thread thread_;
};